 #region Using Statements
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
#endregion

namespace KLib.Physics
{
    /// <summary>
    /// Represents a Generic entity in the Physics System.
    /// <para>It contains no Geometry, it simply updates the positional, rotation, and other values</para>
    /// <para>It is the lowest, most basic part of the Physics System</para>
    /// </summary>
    /// 
#if !XBOX
    [System.ComponentModel.TypeConverter(typeof(CustomPropery<Body>))]
#endif
    public class Body : IDisposable, IPositionable, IRotatable
    {
        #region Fields

        protected bool isStatic = false;
        protected bool ignoresGravity = false;
        protected bool enabled = true;
        protected bool keepUpright = false;
        protected bool isDisposed = false;

        protected float mass = 1.0f;
        protected float inverseMass = 1.0f;
        protected float momentOfInertia = 1.0f;
        protected float inverseMomentOfInertia = 1.0f;

        internal Vector2 position = Vector2.Zero;
        protected float rotation = 0.0f;
        protected float totalRotation = 0.0f;
        protected int rotations = 0;

        protected Vector2 linearVelocity = Vector2.Zero;
        protected float rotationalVelocity = 0.0f;

        protected Vector2 linearVelocityBias = Vector2.Zero;
        protected float rotationalVelocityBias = 0.0f;

        protected Vector2 impulse = Vector2.Zero;
        protected Vector2 force = Vector2.Zero;
        protected float torque = 0.0f;

        protected float linearDragCoefficient = 0.001f;
        protected float rotationalDragCoefficient = 0.001f;

        internal PhysicsManager physicsManager;
        #endregion

        #region Properties

        #region IsStatic | Ignores Gravity | Keep Upright | Enabled | IsDisposed
        /// <summary>
        /// Gets or Sets if the Body is 'Static' meaning that the body does not react.
        /// <para>This means gravity does not effect it, and neither do any forces/objects.</para>
        /// <para>To simply make an object not react to gravity, use IgnoresGravity instead,</para>
        /// </summary>
        public bool IsStatic
        {
            get { return isStatic; }
            set
            {
                isStatic = value;
                if (isStatic)
                {
                    inverseMass = 0f;
                    inverseMomentOfInertia = 0f;
                }
                else
                {
                    inverseMass = 1 / mass;
                    inverseMomentOfInertia = 1 / momentOfInertia;
                }
            }
        }

        /// <summary>
        /// Gets or Sets if the Body reacts to gravity.
        /// <para>If a body ignores gravity, it can still be effected by other forces, to ignore all forces use IsStatic instead.</para>
        /// </summary>
        public bool IgnoresGravity
        {
            get { return ignoresGravity; }
            set { ignoresGravity = value; }
        }

        /// <summary>
        /// Gets or Sets if the Body should react to Rotational forces
        /// </summary>
        public bool KeepUpright
        {
            get { return keepUpright; }
            set { keepUpright = value; }
        }

        /// <summary>
        /// Gets or Sets if this body will be effected by the Physics Manager
        /// </summary>
        public bool Enabled
        {
            get { return enabled; }
            set { enabled = value; }
        }

        public bool IsDisposed
        {
            get { return isDisposed; }
        }
        #endregion

        #region Mass | Inverse Mass | MomentOfInertia | InverseMomentOfInertia
        /// <summary>
        /// Gets or Sets the mass of the Body.
        /// </summary>
        public float Mass
        {
            get { return mass; }
            set
            {
                if (value == 0) { throw new Exception("Mass of a Body cannot be Zero"); }
                mass = value;
                if (!isStatic)
                    inverseMass = 1 / mass;
            }
        }

        /// <summary>
        /// Gets the inverse mass of the Body (Equal to 1/Mass)
        /// </summary>
        public float InverseMass
        {
            get { return inverseMass; }
        }

        /// <summary>
        /// Gets or Sets the Moment of Inertia of the Body.
        /// <para>Moment of Inertia represents how difficult it is to rotate the body around it's center of mass</para>
        /// </summary>
        public float MomentOfInertia
        {
            get { return momentOfInertia; }
            set
            {
                if (value == 0) return;
                momentOfInertia = value;
                if (!isStatic)
                    inverseMomentOfInertia = 1 / momentOfInertia;
            }
        }


        /// <summary>
        /// Gets the Inverse Moment of Inertia of the Body. (Equal to 1/MomentOfInertia)
        /// </summary>
        public float InverseMomentOfIntertia
        {
            get { return inverseMomentOfInertia; }
        }
        #endregion

        #region Position | Rotation | TotalRotation | Rotations

        /// <summary>
        /// Gets or Sets the position of the Body
        /// </summary>
        public Vector2 Position
        {
            get { return position; }
            set { position = value; }
        }


        /// <summary>
        /// Gets or Sets the Rotation (In Radians) of the Body (0 - Pi/2)
        /// </summary>
        public float Rotation
        {
            get { return rotation; }
            set
            {
                rotation = value;
                while (rotation > MathHelper.TwoPi)
                {
                    rotation -= MathHelper.TwoPi;
                    rotations++;
                }
                while (rotations < 0)
                {
                    rotation += MathHelper.TwoPi;
                    rotations--;
                }
                totalRotation = rotation + rotations * MathHelper.TwoPi;
            }
        }

        /// <summary>
        /// Gets the Total Rotation (In Radians) of the Body
        /// </summary>
        public float TotalRotation
        {
            get { return totalRotation; }
        }

        /// <summary>
        /// Gets the number of full rotations the Total Rotation of the Body is describing.
        /// <para>For example, if the total rotation of the object is Pi/2 + 1 radians, this value would be one, and rotation would be 1</para>
        /// </summary>
        public int Rotations
        {
            get { return rotations; }
        }
        #endregion

        #region LinearVelocity | RotationalVelocity | PreviousLinearVelocity | PreviousRotationalVelocity
        /// <summary>
        /// Gets or sets the Linear Velocity of the Body
        /// </summary>
        public Vector2 LinearVelocity
        {
            get { return linearVelocity; }
            set { linearVelocity = value; }
        }

        /// <summary>
        /// Gets or sets the Rotational Velocity of the Body
        /// </summary>
        public float RotationalVelocity
        {
            get { return rotationalVelocity; }
            set { rotationalVelocity = value; }
        }
        #endregion

        #region Impulse | Force | Torque
        /// <summary>
        /// Gets the Inpulse, The change in Momentum from a force over time.
        /// </summary>
        public Vector2 Impulse
        {
            get { return impulse; }
        }

        /// <summary>
        /// Gets the Force on the Body.
        /// </summary>
        public Vector2 Force
        {
            get { return force; }
        }

        /// <summary>
        /// Gets the Torque on the Body. Torque is how much of the force is causeing it to Rotate
        /// </summary>
        public float Torque
        {
            get { return torque; }
        }
        #endregion

        #region LinearDragCoefficient | RotationalDragCoefficent
        /// <summary>
        /// Gets or Sets the Linear Drag Coefficient.
        /// <para>The Linear Drag Coefficient defines how much linear (directional) drag is applied to the Body</para>
        /// </summary>
        public float LinearDragCoefficient
        {
            get { return linearDragCoefficient; }
            set { linearDragCoefficient = value; }

        }

        /// <summary>
        /// Gets or Sets the Rotatioal Drag Coefficient
        /// <para>The Rotational Drag Coefficent defines how much rotational drag is applied to the Body</para>
        /// </summary>
        public float RotationalDragCoefficent
        {
            get { return rotationalDragCoefficient; }
            set { rotationalDragCoefficient = value; }
        }
        #endregion


        #endregion

        #region Initialization

        /// <summary>
        /// Creates a new Empty body
        /// </summary>
        public Body() { }

        /// <summary>
        /// Creates a Copy of another body
        /// </summary>
        /// <param name="body"></param>
        public Body(Body body)
        {
            this.Mass = body.Mass;
            this.MomentOfInertia = body.MomentOfInertia;
            this.LinearDragCoefficient = body.LinearDragCoefficient;
            this.RotationalDragCoefficent = body.RotationalDragCoefficent;
        }
        #endregion

        #region Methods

        #region GetMatrix() | GetRotationMatrix()
        private Matrix translationMatrix = Matrix.Identity;
        private Matrix rotationMatrix = Matrix.Identity;
        private Matrix totalMatrix = Matrix.Identity;

        public Matrix GetMatrix()
        {
            Matrix.CreateTranslation(position.X, position.Y, 0, out translationMatrix);
            Matrix.CreateRotationZ(rotation, out rotationMatrix);
            Matrix.Multiply(ref rotationMatrix, ref translationMatrix, out totalMatrix);
            return totalMatrix;
        }

        public void GetMatrix(out Matrix matrix)
        {
            Matrix.CreateTranslation(position.X, position.Y, 0, out translationMatrix);
            Matrix.CreateRotationZ(rotation, out rotationMatrix);
            Matrix.Multiply(ref rotationMatrix, ref translationMatrix, out matrix);
        }

        public Matrix GetRotationMatrix()
        {
            Matrix.CreateRotationZ(rotation, out rotationMatrix);
            return rotationMatrix;
        }

        public void GetRotationMatrix(out Matrix rotationMatrix)
        {
            Matrix.CreateRotationZ(rotation, out rotationMatrix);
        }
        #endregion

        #region GetWorldPosition() | GetLocalPosition()
        private Vector2 returnPosition = Vector2.Zero;

        /// <summary>
        /// Returns the World Position of the Body
        /// </summary>
        public Vector2 GetWorldPosition(Vector2 localPosition)
        {
            GetMatrix(out totalMatrix);
            Vector2.Transform(ref localPosition, ref totalMatrix, out returnPosition);
            return returnPosition;
        }

        /// <summary>
        /// Returns the World Position of the Body
        /// </summary>
        public void GetWorldPosition(ref Vector2 localPosition, out Vector2 worldPosition)
        {
            GetMatrix(out totalMatrix);
            Vector2.Transform(ref localPosition, ref totalMatrix, out worldPosition);
        }

        public Vector2 GetLocalPosition(Vector2 worldPosition)
        {
            GetRotationMatrix(out rotationMatrix);
            Matrix.Transpose(ref rotationMatrix, out rotationMatrix);
            Vector2.Subtract(ref worldPosition, ref position, out returnPosition);
            Vector2.Transform(ref returnPosition, ref rotationMatrix, out returnPosition);
            return returnPosition;
        }

        public void GetLocalPosition(ref Vector2 worldPosition, out Vector2 localPosition)
        {
            GetRotationMatrix(out rotationMatrix);
            Matrix.Transpose(ref rotationMatrix, out rotationMatrix);
            Vector2.Subtract(ref worldPosition, ref position, out returnPosition);
            Vector2.Transform(ref returnPosition, ref rotationMatrix, out localPosition);
        }
        #endregion

        #region GetVelocityAtLocalPoint() | GetVelocityAtWorldPoint() | GetVelocityAtWorldOffset()
        private Vector2 returnVelocity = Vector2.Zero;

        public Vector2 GetVelocityAtLocalPoint(Vector2 localPoint)
        {
            GetVelocityAtLocalPoint(ref localPoint, out returnVelocity);
            return returnVelocity;
        }

        public void GetVelocityAtLocalPoint(ref Vector2 localPoint, out Vector2 velocity)
        {
            GetWorldPosition(ref localPoint, out returnPosition);
            Vector2.Subtract(ref returnPosition, ref position, out returnPosition);
            GetVelocityAtWorldOffset(ref returnPosition, out velocity);
        }

        public Vector2 GetVelocityAtWorldPoint(Vector2 worldPoint)
        {
            GetVelocityAtWorldPoint(ref worldPoint, out returnVelocity);
            return returnVelocity;
        }

        public void GetVelocityAtWorldPoint(ref Vector2 worldPoint, out Vector2 velocity)
        {
            Vector2.Subtract(ref worldPoint, ref position, out returnPosition);
            GetVelocityAtWorldOffset(ref returnPosition, out velocity);
        }

        public void GetVelocityAtWorldOffset(ref Vector2 offset, out Vector2 velocity)
        {
            velocity = Vector2.Zero;

            //Doing Math the long way here for preformace sake
            //Cross
            velocity.X = -rotationalVelocity * offset.Y;
            velocity.Y = rotationalVelocity * offset.X;

            //Add
            velocity.X = velocity.X + linearVelocity.X;
            velocity.Y = velocity.Y + linearVelocity.Y;
        }

        public void GetVelocityBiasAtWorldOffset(ref Vector2 offset, out Vector2 velocityBias)
        {
            velocityBias = Vector2.Zero;

            //Doing Math the long way here for preformance sake

            //Cross
            velocityBias.X = -rotationalVelocityBias * offset.Y;
            velocityBias.Y = rotationalVelocityBias * offset.X;

            //Add
            velocityBias.X = velocityBias.X + linearVelocityBias.X;
            velocityBias.Y = velocityBias.Y + linearVelocityBias.Y;
        }
        #endregion

        #region ApplyForce() | ApplyForceAtPoint()
        /// <summary>
        /// Applies a force to the entire body
        /// </summary>
        public void ApplyForce(Vector2 force)
        {
            this.force.X = this.force.X + force.X;
            this.force.Y = this.force.Y + force.Y;
        }
        /// <summary>
        /// Applies a force to the entire body
        /// </summary>
        public void ApplyForce(ref Vector2 force)
        {
            this.force.X = this.force.X + force.X;
            this.force.Y = this.force.Y + force.Y;
        }

        private Vector2 difference = Vector2.Zero;
        /// <summary>
        /// Applies a force to a specific part of the body. This differs from ApplyForce() because it
        /// also considers what rotational force would be applied from a force at the specified point
        /// </summary>
        /// <param name="force">How much force is to be applied</param>
        /// <param name="point">The part of the Body to apply the force to</param>
        public void ApplyForceAtLocalPoint(Vector2 force, Vector2 point)
        {
            GetWorldPosition(ref point, out difference);
            Vector2.Subtract(ref difference, ref position, out difference);

            float torque = difference.X * force.Y - difference.Y * force.X;
            this.torque += torque;

            this.force.X += force.X;
            this.force.Y += force.Y;
        }

        /// <summary>
        /// Applies a force to a specific part of the body. This differs from ApplyForce() because it
        /// also considers what rotational force would be applied from a force at the specified point
        /// </summary>
        /// <param name="force">How much force is to be applied</param>
        /// <param name="point">The part of the Body to apply the force to</param>
        public void ApplyForceAtLocalPoint(ref Vector2 force, ref Vector2 point)
        {
            GetWorldPosition(ref point, out difference);
            Vector2.Subtract(ref difference, ref position, out difference);

            float torque = difference.X * force.Y - difference.Y * force.X;
            this.torque += torque;

            this.force.X += force.X;
            this.force.Y += force.Y;
        }
        #endregion

        #region ClearForce()
        /// <summary>
        /// Removes all forces on the Body
        /// </summary>
        public void ClearForce()
        {
            force.X = 0.0f;
            force.Y = 0.0f;
        }
        #endregion

        #region ApplyTorque() | ClearTorque()

        /// <summary>
        /// Applies torque to the Body
        /// </summary>
        public void ApplyTorque(float torque)
        {
            this.torque += torque;
        }

        /// <summary>
        /// Applies torque to the Body
        /// </summary>
        public void ApplyTorque(ref float torque)
        {
            this.torque += torque;
        }

        /// <summary>
        /// Removes the Torque on the Body
        /// </summary>
        public void ClearTorque()
        {
            this.torque = 0.0f;
        }
        #endregion

        #region ApplyImpulse() | ApplyImmidiateImpulse() | ClearImpulses() | ApplyImpulses()

        private Vector2 deltaVelocity = Vector2.Zero;

        /// <summary>
        /// Applies an Impulse that will be collected with all other impulses and applied as a group
        /// </summary>
        public void ApplyImpulse(Vector2 impulse)
        {
            //Multiply
            deltaVelocity.X = impulse.X * inverseMass;
            deltaVelocity.Y = impulse.Y * InverseMass;

            //Add
            this.impulse.X += deltaVelocity.X + linearVelocity.X;
            this.impulse.Y += deltaVelocity.Y + linearVelocity.Y;
        }

        /// <summary>
        /// Applies an Impulse that will be collected with all other impulses and applied as a group
        /// </summary>
        public void ApplyImpulse(ref Vector2 impulse)
        {
            //Multiply
            deltaVelocity.X = impulse.X * inverseMass;
            deltaVelocity.Y = impulse.Y * InverseMass;

            //Add
            this.impulse.X += deltaVelocity.X + linearVelocity.X;
            this.impulse.Y += deltaVelocity.Y + linearVelocity.Y;
        }

        /// <summary>
        /// Applies all Impulses that have been collected
        /// </summary>
        internal void ApplyImpulses()
        {
            ApplyImmidiateImpulse(ref this.impulse);
            this.impulse.X = 0.0f;
            this.impulse.Y = 0.0f;
        }

        /// <summary>
        /// Applies the impulse imidiatly, without the rest of the group
        /// </summary>
        protected void ApplyImmidiateImpulse(ref Vector2 impulse)
        {
            //Multiply
            deltaVelocity.X = impulse.X * inverseMass;
            deltaVelocity.Y = impulse.Y * inverseMass;

            //Add
            linearVelocity.X = impulse.X + linearVelocity.X;
            linearVelocity.Y = impulse.Y + linearVelocity.Y;
        }

        /// <summary>
        /// Sets the group of impulses to Vector2.Zero
        /// </summary>
        public void ClearImpulses()
        {
            impulse.X = 0.0f;
            impulse.Y = 0.0f;
        }
        #endregion

        #region ApplyDrag()

        private Vector2 linearDrag = Vector2.Zero;
        private float rotationalDrag = 0.0f;
        public void ApplyDrag()
        {
            linearDrag.X = -linearVelocity.X * linearDragCoefficient;
            linearDrag.Y = -linearVelocity.Y * linearDragCoefficient;

            try
            {
                rotationalDrag = rotationalVelocity * rotationalVelocity * Math.Sign(rotationalVelocity);
            }
            catch { }

            rotationalDrag *= -rotationalDragCoefficient;

            ApplyForce(ref linearDrag);
            ApplyTorque(ref rotationalDrag);
        }
        #endregion

        #region CalculateVelocity() | CalculatePosition()
        //private Vector2 deltaVelocity = Vector2.Zero; (Defined in ApplyImpulse() region but used again here)
        private Vector2 acceleration = Vector2.Zero;
        private float deltaRotationalVelocity = 0.0f;

        internal void CalculateVelocity(float time)
        {
            if (isStatic)
                return;

            #region Linear Velocity
            ApplyDrag();

            acceleration.X = force.X * inverseMass;
            acceleration.Y = force.Y * inverseMass;

            deltaVelocity.X = acceleration.X * time;
            deltaVelocity.Y = acceleration.Y * time;

            linearVelocity.X += deltaVelocity.X;
            linearVelocity.Y += deltaVelocity.Y;

            /*if ((float)Math.Abs(linearVelocity.X) < 5f)
                linearVelocity.X = 0f;
            if ((float)Math.Abs(linearVelocity.Y) < 5f)
                linearVelocity.Y = 0f;*/
            #endregion

            #region Rotational Velocity

            if (KeepUpright)
                return;

            deltaRotationalVelocity = torque * inverseMomentOfInertia * time;
            rotationalVelocity += deltaRotationalVelocity;

            if ((float)Math.Abs(rotationalVelocity) < 0.1f)
                rotationalVelocity = 0f;
            #endregion
        }

        private Vector2 deltaPosition = Vector2.Zero;
        private float deltaRotation = 0.0f;

        internal void CalculatePosition(float time)
        {
            if (isStatic)
                return;

            #region Linear Position
            deltaPosition.X = (linearVelocity.X + linearVelocityBias.X) * time;
            deltaPosition.Y = (linearVelocity.Y + linearVelocityBias.Y) * time;

            //position.X += deltaPosition.X;
            //position.Y += deltaPosition.Y;
            Move(deltaPosition);
            #endregion

            #region Rotation
                deltaRotation = (rotationalVelocity + rotationalVelocityBias) * time;
                //Rotation = rotation + deltaRotation;
                Rotate(deltaRotation);
            #endregion

            linearVelocityBias.X = 0.0f;
            linearVelocityBias.Y = 0.0f;
            rotationalVelocityBias = 0.0f;
        }
        #endregion

        #region ApplyImpulseAtWorldOffset() | ApplyImpulseBiasAtWorldOffset()

        public void ApplyImpulseAtWorldOffset(ref Vector2 impulse, ref Vector2 offset)
        {
            #region Linear
            deltaVelocity.X = impulse.X * inverseMass;
            deltaVelocity.Y = impulse.Y * inverseMass;

            linearVelocity.X += deltaVelocity.X;
            linearVelocity.Y += deltaVelocity.Y;
            #endregion

            if (KeepUpright)
                return;

            #region Rotational
            float rotationalImpulse = offset.X * impulse.Y - offset.Y * impulse.X;
            rotationalImpulse *= inverseMomentOfInertia;
            rotationalVelocity += rotationalImpulse;
            #endregion

        }

        public void ApplyImpulseBiasAtWorldOffset(ref Vector2 impulseBias, ref Vector2 offset)
        {
            #region Linear
            deltaVelocity.X = impulseBias.X * inverseMass;
            deltaVelocity.Y = impulseBias.Y * inverseMass;

            linearVelocityBias.X += deltaVelocity.X;
            linearVelocityBias.Y += deltaVelocity.Y;
            #endregion

            if (KeepUpright)
                return;

            #region Rotational
            float rotationalImpulseBias = offset.X * impulseBias.Y - offset.Y * impulseBias.X;
            rotationalImpulseBias *= inverseMomentOfInertia;
            rotationalVelocityBias += rotationalImpulseBias;
            #endregion
        }

        #endregion

        #region Dispose()
        public void Dispose()
        {
            physicsManager.RemoveBody(this);
            isDisposed = true;
            GC.SuppressFinalize(this);
        }
        #endregion

        #region IPositionable Methods
        public virtual void Move(Vector2 amount)
        {
            position.X += amount.X;
            position.Y += amount.Y;
        }

        public virtual void MoveTo(Vector2 position)
        {
            this.position.X = position.X;
            this.position.Y = position.Y;
        }
        #endregion

        #region IRotatable Methods
        public virtual void Rotate(float radians)
        {
            Rotation = Rotation + radians;
        }

        public virtual void RotateTo(float radians)
        {
            Rotation = radians;
        }
        #endregion

        #region Update
        public virtual void Update()
        {
        }
        #endregion

        #endregion

        #region Helper Methods
        #endregion
    }
}
